package com.spukmk3me.math;

/* $if SPUKMK3ME_DEBUG$ */
import com.spukmk3me.debug.Logger;
/* $endif$ */

/**
 *  Simple implement of Matrix4f.
 *  \details Data is stored as column-major matrix.
 */
public class SimpleMatrix4f implements Matrix4f
{
    /**
     *  Create an identity matrix.
     */
    public SimpleMatrix4f()
    {
        m_data = new float[ 16 ];
        reset();
    }
    
    public SimpleMatrix4f( float[] data, int offset )
    {
        m_data = new float[ 16 ];
        setRawData( data, offset );
    }
    
    public SimpleMatrix4f( float[] data )
    {
        this( data, 0 );
    }
    
    public SimpleMatrix4f( Matrix4f matrix )
    {
        this( matrix.getRawData() );
    }
    
    @Override
    public void copy( Matrix4f matrix )
    {
        setRawData( matrix.getRawData(), 0 );
    }
    
    @Override
    public void add( Matrix4f matrix )
    {
        float[] m = matrix.getRawData();
        
        for ( int i = 0; i != 16; ++i )
            m_data[ i ] += m[ i ];
    }
    
    @Override
    public void mul( float n )
    {
        for ( int i = 0; i != 16; ++i )
            m_data[ i ] *= n;
    }
    
    @Override
    public void mul( Matrix4f matrix )
    {
        mul( matrix.getRawData(), true );
    }
    
    @Override
    public void mul_inv( Matrix4f matrix )
    {
        mul( matrix.getRawData(), false );
    }
    
    /**
     *  
     *   @param matrix
     *   @param inverse True: Do (this x matrix), which means (matrix x this) in
     *  mathematics as the data is column-major
     */
    private void mul( float[] matrix, boolean inverse )
    {
        if ( m_tempMul == null )
            m_tempMul = new float[ 16 ];
        
        float[] m1 = ( inverse )? matrix : m_data;
        float[] m2 = ( inverse )? m_data : matrix;
        float   temp;
        int     matrix1Index = 0, matrix2Index = 0, j;
        
        for ( int i = 0; i != 16; ++i )
        {
            //System.out.println( "Matrix index: " + matrix1Index + " " + matrix2Index );
            temp = 0;
            
            for ( j = 4; j != 0; --j )
            {
                temp += m1[ matrix1Index ] * m2[ matrix2Index ];
                ++matrix1Index;
                matrix2Index += 4;
            }
            
            if ( ++matrix2Index < 20 )
                matrix1Index -= 4;
            
            matrix2Index &= 0x03;
            m_tempMul[ i ] = temp;
        }
        
        setRawData( m_tempMul, 0 );
    }
    
    /**
     *  Scale function.
     * @param x
     * @param y
     * @param z
     */
    @Override
    public void s( float x, float y, float z )
    {
        m_data[ 0 ]    *= x;
        m_data[ 4 ]    *= x;
        m_data[ 8 ]    *= x;
        m_data[ 12 ]   *= x;
        
        m_data[ 1 ]    *= y;
        m_data[ 5 ]    *= y;
        m_data[ 9 ]    *= y;
        m_data[ 13 ]   *= y;
        
        m_data[ 2 ]    *= z;
        m_data[ 6 ]    *= z;
        m_data[ 10 ]   *= z;
        m_data[ 14 ]   *= z;
    }
    
    /**
     *  Translate function.
     * @param x
     * @param y
     * @param z
     */
    @Override
    public void t( float x, float y, float z )
    {
        float temp;
        
        temp            = m_data[ 3 ];
        m_data[ 0 ]    += temp * x;
        m_data[ 1 ]    += temp * y;
        m_data[ 2 ]    += temp * z;
        
        temp            = m_data[ 7 ];
        m_data[ 4 ]    += temp * x;
        m_data[ 5 ]    += temp * y;
        m_data[ 6 ]    += temp * z;
        
        temp            = m_data[ 11 ];
        m_data[ 8 ]    += temp * x;
        m_data[ 9 ]    += temp * y;
        m_data[ 10 ]   += temp * z;
        
        temp            = m_data[ 15 ];
        m_data[ 12 ]   += temp * x;
        m_data[ 13 ]   += temp * y;
        m_data[ 14 ]   += temp * z;
    }
    
    /**
     *  Rotate function
     *  \details x-y-z convention.
     */
    @Override
    public void r( float x, float y, float z, float angle )
    {
        if ( m_tempRot == null )
            m_tempRot = new float[ 16 ];
        
        float sin       = (float)Math.sin( angle );
        float cos       = (float)Math.cos( angle );
        float length    = (float)Math.sqrt( x * x + y * y + z * z );
        
        x /= length;
        y /= length;
        z /= length;
        
        m_tempRot[ 0 ]      = x * x * (1 - cos) + cos;
        m_tempRot[ 4 ]      = x * y * (1 - cos) - z * sin;
        m_tempRot[ 8 ]      = x * z * (1 - cos) + y * sin;
        m_tempRot[ 12 ]     = 0;
        m_tempRot[ 1 ]      = x * y * (1 - cos) + z * sin;
        m_tempRot[ 5 ]      = y * y * (1 - cos) + cos;
        m_tempRot[ 9 ]      = y * z * (1 - cos) - x * sin;
        m_tempRot[ 13 ]     = 0;
        m_tempRot[ 2 ]      = x * z * (1 - cos) - y * sin;
        m_tempRot[ 6 ]      = y * z * (1 - cos) + x * sin;
        m_tempRot[ 10 ]     = z * z * (1 - cos) + cos;
        m_tempRot[ 14 ]     = m_tempRot[ 3 ] = m_tempRot[ 7 ] = m_tempRot[ 11 ] = 0;
        m_tempRot[ 15 ]     = 1;
        
        mul( m_tempRot, false );
    }
    
    /**
     *  Inverts the matrix. Throws a RuntimeException in case the matrix is not invertible. Stores the result in this matrix.
     *  \details Copied from LibGDX, class Matrix4.
     */
    @Override
    public void invert()
    {
        float l_det = m_data[ 3 ] * m_data[ 6 ] * m_data[ 9 ] * m_data[ 12 ] - m_data[ 2 ] * m_data[ 7 ] * m_data[ 9 ] * m_data[ 12 ] - m_data[ 3 ] * m_data[ 5 ]
            * m_data[ 10 ] * m_data[ 12 ] + m_data[ 1 ] * m_data[ 7 ] * m_data[ 10 ] * m_data[ 12 ] + m_data[ 2 ] * m_data[ 5 ] * m_data[ 11 ] * m_data[ 12 ] - m_data[ 1 ]
            * m_data[ 6 ] * m_data[ 11 ] * m_data[ 12 ] - m_data[ 3 ] * m_data[ 6 ] * m_data[ 8 ] * m_data[ 13 ] + m_data[ 2 ] * m_data[ 7 ] * m_data[ 8 ] * m_data[ 13 ]
            + m_data[ 3 ] * m_data[ 4 ] * m_data[ 10 ] * m_data[ 13 ] - m_data[ 0 ] * m_data[ 7 ] * m_data[ 10 ] * m_data[ 13 ] - m_data[ 2 ] * m_data[ 4 ] * m_data[ 11 ]
            * m_data[ 13 ] + m_data[ 0 ] * m_data[ 6 ] * m_data[ 11 ] * m_data[ 13 ] + m_data[ 3 ] * m_data[ 5 ] * m_data[ 8 ] * m_data[ 14 ] - m_data[ 1 ] * m_data[ 7 ]
            * m_data[ 8 ] * m_data[ 14 ] - m_data[ 3 ] * m_data[ 4 ] * m_data[ 9 ] * m_data[ 14 ] + m_data[ 0 ] * m_data[ 7 ] * m_data[ 9 ] * m_data[ 14 ] + m_data[ 1 ]
            * m_data[ 4 ] * m_data[ 11 ] * m_data[ 14 ] - m_data[ 0 ] * m_data[ 5 ] * m_data[ 11 ] * m_data[ 14 ] - m_data[ 2 ] * m_data[ 5 ] * m_data[ 8 ] * m_data[ 15 ]
            + m_data[ 1 ] * m_data[ 6 ] * m_data[ 8 ] * m_data[ 15 ] + m_data[ 2 ] * m_data[ 4 ] * m_data[ 9 ] * m_data[ 15 ] - m_data[ 0 ] * m_data[ 6 ] * m_data[ 9 ]
            * m_data[ 15 ] - m_data[ 1 ] * m_data[ 4 ] * m_data[ 10 ] * m_data[ 15 ] + m_data[ 0 ] * m_data[ 5 ] * m_data[ 10 ] * m_data[ 15 ];
        
        if ( l_det == 0f )
            throw new RuntimeException( "non-invertible matrix" );
        
        float inv_det = 1.0f / l_det;
        float[] tmp = new float[ 16 ];
        
        tmp[ 0 ] = m_data[ 9 ] * m_data[ 14 ] * m_data[ 7 ] - m_data[ 13 ] * m_data[ 10 ] * m_data[ 7 ] + m_data[ 13 ] * m_data[ 6 ] * m_data[ 11 ] - m_data[ 5 ]
            * m_data[ 14 ] * m_data[ 11 ] - m_data[ 9 ] * m_data[ 6 ] * m_data[ 15 ] + m_data[ 5 ] * m_data[ 10 ] * m_data[ 15 ];
        tmp[ 4 ] = m_data[ 12 ] * m_data[ 10 ] * m_data[ 7 ] - m_data[ 8 ] * m_data[ 14 ] * m_data[ 7 ] - m_data[ 12 ] * m_data[ 6 ] * m_data[ 11 ] + m_data[ 4 ]
            * m_data[ 14 ] * m_data[ 11 ] + m_data[ 8 ] * m_data[ 6 ] * m_data[ 15 ] - m_data[ 4 ] * m_data[ 10 ] * m_data[ 15 ];
        tmp[ 8 ] = m_data[ 8 ] * m_data[ 13 ] * m_data[ 7 ] - m_data[ 12 ] * m_data[ 9 ] * m_data[ 7 ] + m_data[ 12 ] * m_data[ 5 ] * m_data[ 11 ] - m_data[ 4 ]
            * m_data[ 13 ] * m_data[ 11 ] - m_data[ 8 ] * m_data[ 5 ] * m_data[ 15 ] + m_data[ 4 ] * m_data[ 9 ] * m_data[ 15 ];
        tmp[ 12 ] = m_data[ 12 ] * m_data[ 9 ] * m_data[ 6 ] - m_data[ 8 ] * m_data[ 13 ] * m_data[ 6 ] - m_data[ 12 ] * m_data[ 5 ] * m_data[ 10 ] + m_data[ 4 ]
            * m_data[ 13 ] * m_data[ 10 ] + m_data[ 8 ] * m_data[ 5 ] * m_data[ 14 ] - m_data[ 4 ] * m_data[ 9 ] * m_data[ 14 ];
        tmp[ 1 ] = m_data[ 13 ] * m_data[ 10 ] * m_data[ 3 ] - m_data[ 9 ] * m_data[ 14 ] * m_data[ 3 ] - m_data[ 13 ] * m_data[ 2 ] * m_data[ 11 ] + m_data[ 1 ]
            * m_data[ 14 ] * m_data[ 11 ] + m_data[ 9 ] * m_data[ 2 ] * m_data[ 15 ] - m_data[ 1 ] * m_data[ 10 ] * m_data[ 15 ];
        tmp[ 5 ] = m_data[ 8 ] * m_data[ 14 ] * m_data[ 3 ] - m_data[ 12 ] * m_data[ 10 ] * m_data[ 3 ] + m_data[ 12 ] * m_data[ 2 ] * m_data[ 11 ] - m_data[ 0 ]
            * m_data[ 14 ] * m_data[ 11 ] - m_data[ 8 ] * m_data[ 2 ] * m_data[ 15 ] + m_data[ 0 ] * m_data[ 10 ] * m_data[ 15 ];
        tmp[ 9 ] = m_data[ 12 ] * m_data[ 9 ] * m_data[ 3 ] - m_data[ 8 ] * m_data[ 13 ] * m_data[ 3 ] - m_data[ 12 ] * m_data[ 1 ] * m_data[ 11 ] + m_data[ 0 ]
            * m_data[ 13 ] * m_data[ 11 ] + m_data[ 8 ] * m_data[ 1 ] * m_data[ 15 ] - m_data[ 0 ] * m_data[ 9 ] * m_data[ 15 ];
        tmp[ 13 ] = m_data[ 8 ] * m_data[ 13 ] * m_data[ 2 ] - m_data[ 12 ] * m_data[ 9 ] * m_data[ 2 ] + m_data[ 12 ] * m_data[ 1 ] * m_data[ 10 ] - m_data[ 0 ]
            * m_data[ 13 ] * m_data[ 10 ] - m_data[ 8 ] * m_data[ 1 ] * m_data[ 14 ] + m_data[ 0 ] * m_data[ 9 ] * m_data[ 14 ];
        tmp[ 2 ] = m_data[ 5 ] * m_data[ 14 ] * m_data[ 3 ] - m_data[ 13 ] * m_data[ 6 ] * m_data[ 3 ] + m_data[ 13 ] * m_data[ 2 ] * m_data[ 7 ] - m_data[ 1 ]
            * m_data[ 14 ] * m_data[ 7 ] - m_data[ 5 ] * m_data[ 2 ] * m_data[ 15 ] + m_data[ 1 ] * m_data[ 6 ] * m_data[ 15 ];
        tmp[ 6 ] = m_data[ 12 ] * m_data[ 6 ] * m_data[ 3 ] - m_data[ 4 ] * m_data[ 14 ] * m_data[ 3 ] - m_data[ 12 ] * m_data[ 2 ] * m_data[ 7 ] + m_data[ 0 ]
            * m_data[ 14 ] * m_data[ 7 ] + m_data[ 4 ] * m_data[ 2 ] * m_data[ 15 ] - m_data[ 0 ] * m_data[ 6 ] * m_data[ 15 ];
        tmp[ 10 ] = m_data[ 4 ] * m_data[ 13 ] * m_data[ 3 ] - m_data[ 12 ] * m_data[ 5 ] * m_data[ 3 ] + m_data[ 12 ] * m_data[ 1 ] * m_data[ 7 ] - m_data[ 0 ]
            * m_data[ 13 ] * m_data[ 7 ] - m_data[ 4 ] * m_data[ 1 ] * m_data[ 15 ] + m_data[ 0 ] * m_data[ 5 ] * m_data[ 15 ];
        tmp[ 14 ] = m_data[ 12 ] * m_data[ 5 ] * m_data[ 2 ] - m_data[ 4 ] * m_data[ 13 ] * m_data[ 2 ] - m_data[ 12 ] * m_data[ 1 ] * m_data[ 6 ] + m_data[ 0 ]
            * m_data[ 13 ] * m_data[ 6 ] + m_data[ 4 ] * m_data[ 1 ] * m_data[ 14 ] - m_data[ 0 ] * m_data[ 5 ] * m_data[ 14 ];
        tmp[ 3 ] = m_data[ 9 ] * m_data[ 6 ] * m_data[ 3 ] - m_data[ 5 ] * m_data[ 10 ] * m_data[ 3 ] - m_data[ 9 ] * m_data[ 2 ] * m_data[ 7 ] + m_data[ 1 ]
            * m_data[ 10 ] * m_data[ 7 ] + m_data[ 5 ] * m_data[ 2 ] * m_data[ 11 ] - m_data[ 1 ] * m_data[ 6 ] * m_data[ 11 ];
        tmp[ 7 ] = m_data[ 4 ] * m_data[ 10 ] * m_data[ 3 ] - m_data[ 8 ] * m_data[ 6 ] * m_data[ 3 ] + m_data[ 8 ] * m_data[ 2 ] * m_data[ 7 ] - m_data[ 0 ]
            * m_data[ 10 ] * m_data[ 7 ] - m_data[ 4 ] * m_data[ 2 ] * m_data[ 11 ] + m_data[ 0 ] * m_data[ 6 ] * m_data[ 11 ];
        tmp[ 11 ] = m_data[ 8 ] * m_data[ 5 ] * m_data[ 3 ] - m_data[ 4 ] * m_data[ 9 ] * m_data[ 3 ] - m_data[ 8 ] * m_data[ 1 ] * m_data[ 7 ] + m_data[ 0 ]
            * m_data[ 9 ] * m_data[ 7 ] + m_data[ 4 ] * m_data[ 1 ] * m_data[ 11 ] - m_data[ 0 ] * m_data[ 5 ] * m_data[ 11 ];
        tmp[ 15 ] = m_data[ 4 ] * m_data[ 9 ] * m_data[ 2 ] - m_data[ 8 ] * m_data[ 5 ] * m_data[ 2 ] + m_data[ 8 ] * m_data[ 1 ] * m_data[ 6 ] - m_data[ 0 ]
            * m_data[ 9 ] * m_data[ 6 ] - m_data[ 4 ] * m_data[ 1 ] * m_data[ 10 ] + m_data[ 0 ] * m_data[ 5 ] * m_data[ 10 ];
        m_data[ 0 ] = tmp[ 0 ] * inv_det;
        m_data[ 4 ] = tmp[ 4 ] * inv_det;
        m_data[ 8 ] = tmp[ 8 ] * inv_det;
        m_data[ 12 ] = tmp[ 12 ] * inv_det;
        m_data[ 1 ] = tmp[ 1 ] * inv_det;
        m_data[ 5 ] = tmp[ 5 ] * inv_det;
        m_data[ 9 ] = tmp[ 9 ] * inv_det;
        m_data[ 13 ] = tmp[ 13 ] * inv_det;
        m_data[ 2 ] = tmp[ 2 ] * inv_det;
        m_data[ 6 ] = tmp[ 6 ] * inv_det;
        m_data[ 10 ] = tmp[ 10 ] * inv_det;
        m_data[ 14 ] = tmp[ 14 ] * inv_det;
        m_data[ 3 ] = tmp[ 3 ] * inv_det;
        m_data[ 7 ] = tmp[ 7 ] * inv_det;
        m_data[ 11 ] = tmp[ 11 ] * inv_det;
        m_data[ 15 ] = tmp[ 15 ] * inv_det;
    }

    @Override
    public float[] getRawData()
    {
        return m_data;
    }
    
    @Override
    public void setRawData( float[] data, int offset )
    {
        /* $if SPUKMK3ME_DEBUG$ */
        if ( data == null )
            Logger.trace( "Null data." );
        else if ( data.length - offset < 16 )
            Logger.trace( "Not enough data." );
        /* $endif$ */

        System.arraycopy( data, offset, m_data, 0, 16 );
    }
    
    /**
     *  Load the identity (1) matrix.
     */
    @Override
    public void reset()
    {
        System.arraycopy( IDENTITY, 0, m_data, 0, 16 );
    }
    
    @Override
    public boolean hasTranslation()
    {
        return (m_data[ 12 ] != 0f) || (m_data[ 13 ] != 0f) || (m_data[ 14 ] != 0f);
    }
    
    @Override
    public boolean hasRotation()
    {
        return  (m_data[ 1 ] != 0f) || (m_data[ 2 ] != 0f) ||
                (m_data[ 4 ] != 0f) || (m_data[ 6 ] != 0f) ||
                (m_data[ 8 ] != 0f) || (m_data[ 9 ] != 0f);
    }
    
    @Override
    public boolean hasScaling()
    {
        return (m_data[ 0 ] != 0f) || (m_data[ 5 ] != 0f) || (m_data[ 10 ] != 0f);
    }
    
    /* $if SPUKMK3ME_DEBUG$ */
    public void _print()
    {
        for ( int i = 0; i != 4; ++i )
        {
            for ( int j = 0; j != 4; ++j )
                Logger.log( m_data[ i * 4 + j ] + " " );
            
            Logger.log( "\n" );
        }
    }
    /* $endif$ */
    
    private static final float[] IDENTITY = {
        1.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 1.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 1.0f
    };
    
    private float[]     m_data, m_tempMul, m_tempRot;
}
